/*
给红外发送指令，需要轮询该接口回复的数据
*/
#include "bsp_uart.h"
#include "bsp_pthread.h"
#include "gpioconfig.h"
#include "config.h"
#include <stdio.h>
#include <stdint.h>
#define SEND_INTERVAL 1000  // 发送间隔（毫秒）

// 发送命令
void general_send_command(Uart_t* uart, unsigned char* cmd, int len) 
{
    int ret = write(uart->fd, cmd, len);
    if (ret != len) 
    {
        printf("命令发送失败，预期发送 %d 字节，实际发送 %d 字节\n", len, ret);
    } 
    else 
    {
        printf("成功发送激光雷达命令（十六进制）: ");
        for (int i = 0; i < len; i++) 
        {
            printf("%02X ", cmd[i]);
        }
        printf("\n");
    }
}

// 解析相机本地时间
uint64_t parse_camera_time(const unsigned char* buf) {
    uint64_t seconds = ((uint64_t)buf[4] << 24) | 
                      ((uint64_t)buf[5] << 16) | 
                      ((uint64_t)buf[6] << 8) | 
                       (uint64_t)buf[7];
                       
    uint32_t microseconds = ((uint32_t)buf[8] << 16) | 
                           ((uint32_t)buf[9] << 8) | 
                            (uint32_t)buf[10];
                            
    return (seconds * 1000000) + microseconds;
}

// 解析状态字节
void parse_status_byte(unsigned char status, 
                      char* detector_state, 
                      char* video_state,
                      char* camera_state, 
                      char* datalink_state) {
    // 解析探测器上下电状态
    switch ((status >> 6) & 0x03) {
        case 0x01: sprintf(detector_state, "下电状态"); break;
        case 0x02: sprintf(detector_state, "上电状态"); break;
        default: sprintf(detector_state, "未知状态"); break;
    }
    
    // 解析视频处理电路上下电状态
    switch ((status >> 4) & 0x03) {
        case 0x01: sprintf(video_state, "下电状态"); break;
        case 0x02: sprintf(video_state, "上电状态"); break;
        default: sprintf(video_state, "未知状态"); break;
    }
    
    // 解析摄像状态
    switch ((status >> 2) & 0x03) {
        case 0x01: sprintf(camera_state, "停止摄像"); break;
        case 0x02: sprintf(camera_state, "摄像中"); break;
        default: sprintf(camera_state, "未知状态"); break;
    }
    
    // 解析数传上下电状态
    switch (status & 0x03) {
        case 0x01: sprintf(datalink_state, "下电状态"); break;
        case 0x02: sprintf(datalink_state, "上电状态"); break;
        default: sprintf(datalink_state, "未知状态"); break;
    }
}

void general_process_response(unsigned char* buf, int len, int start_index) {
    // 检查起始位置是否有效
    if (start_index < 0 || start_index >= len - 1) {
        printf("错误：起始索引无效\n");
        return;
    }
    
    // 寻找同步码
    int sync_index = -1;
    for (int i = start_index; i < len - 1; i++) {
        if (buf[i] == 0x55 && buf[i+1] == 0xAA) {
            sync_index = i;
            break;
        }
    }
    
    if (sync_index == -1) {
        printf("错误：未找到有效的同步码\n");
        return;
    }
    
    // 计算实际数据帧起始位置
    int frame_start = sync_index;
    
    // 验证帧长度
    if (frame_start + 54 > len) {
        printf("错误：数据帧长度不足\n");
        return;
    }
    
    // 验证地址码
    if (buf[frame_start + 2] != 0x73) {
        printf("错误：地址码校验失败\n");
        return;
    }
    
    // 验证标识码
    if (buf[frame_start + 3] != 0xD7) {
        printf("错误：标识码校验失败\n");
        return;
    }
    
    // 计算并验证验证码
    unsigned char checksum = 0;
    for (int i = frame_start + 2; i < frame_start + 53; i++) {
        checksum += buf[i];
    }
    
    if (checksum != buf[frame_start + 53]) {
        printf("错误：验证码校验失败\n");
        return;
    }
    
    // 解析相机本地时间
    uint64_t camera_time = parse_camera_time(buf + frame_start);
    uint64_t seconds = camera_time / 1000000;
    uint64_t microseconds = camera_time % 1000000;
    
    // 解析红外焦面图像输出模式
    const char* output_mode;
    switch (buf[frame_start + 11]) {
        case 0x30: output_mode = "红外图像模式"; break;
        case 0x31: output_mode = "红外自校1模式"; break;
        case 0x32: output_mode = "红外自校2模式"; break;
        case 0x33: output_mode = "红外自校3模式"; break;
        default: output_mode = "未知模式"; break;
    }
    
    // 解析状态字节
    char detector_state[20], video_state[20], camera_state[20], datalink_state[20];
    parse_status_byte(buf[frame_start + 12], detector_state, video_state, camera_state, datalink_state);
    
    // 解析数据有效信号选择
    const char* signal_source = (buf[frame_start + 13] == 0x0F) ? "测试输入" : "探测器视频信号";
    
    // 解析图像旋转选择
    const char* rotation_mode;
    switch (buf[frame_start + 14]) {
        case 0x00: rotation_mode = "R_C 11"; break;
        case 0x01: rotation_mode = "R_C 00"; break;
        case 0x02: rotation_mode = "R_C 01"; break;
        case 0x03: rotation_mode = "R_C 10"; break;
        default: rotation_mode = "未知模式"; break;
    }
    
    // 解析温度码值
    uint16_t temperature_code = (buf[frame_start + 15] << 8) | buf[frame_start + 16];
    
    // 解析红外帧周期
    uint32_t frame_period = (buf[frame_start + 17] << 16) | (buf[frame_start + 18] << 8) | buf[frame_start + 19];
    
    // 解析通道采样位置
    uint8_t channel_positions[4] = {buf[frame_start + 20], buf[frame_start + 21], 
                                   buf[frame_start + 22], buf[frame_start + 23]};
    
    // 解析红外积分时间
    uint32_t integration_time = (buf[frame_start + 24] << 16) | (buf[frame_start + 25] << 8) | buf[frame_start + 26];
    
    // 解析控温目标
    uint16_t temperature_target = (buf[frame_start + 27] << 8) | buf[frame_start + 28];
    
    // 解析指令状态
    const char* burst_cmd_type = (buf[frame_start + 29] == 0x00) ? "空闲" : "处理中";
    const char* periodic_cmd_type = (buf[frame_start + 31] == 0x00) ? "空闲" : "处理中";
    
    // 解析Serdat设置状态和程序版本
    uint8_t serdat_state = buf[frame_start + 52];
    uint8_t program_version = buf[frame_start + 53];
    
    // 打印解析结果
    printf("在位置 %d 找到数据帧\n", frame_start);
    printf("红外焦面处理工况数据帧解析结果:\n");
    printf("  相机本地时间: %lu秒 %lu微秒\n", seconds, microseconds);
    printf("  图像输出模式: %s\n", output_mode);
    printf("  探测器状态: %s\n", detector_state);
    printf("  视频电路状态: %s\n", video_state);
    printf("  摄像状态: %s\n", camera_state);
    printf("  数传状态: %s\n", datalink_state);
    printf("  数据信号源: %s\n", signal_source);
    printf("  图像旋转模式: %s\n", rotation_mode);
    printf("  温度码值: 0x%04X\n", temperature_code);
    printf("  帧周期: %u微秒\n", frame_period);
    printf("  通道采样位置: %u, %u, %u, %u\n", 
           channel_positions[0], channel_positions[1], 
           channel_positions[2], channel_positions[3]);
    printf("  积分时间: %u微秒\n", integration_time);
    printf("  控温目标: 0x%04X\n", temperature_target);
    printf("  突发指令状态: %s, 计数: %u\n", burst_cmd_type, buf[frame_start + 30]);
    printf("  周期指令状态: %s, 计数: %u\n", periodic_cmd_type, buf[frame_start + 32]);
    printf("  Serdat设置状态: 0x%02X\n", serdat_state);
    printf("  程序版本: 0x%02X\n", program_version);
}

int main(int argc, char **argv)
{
    /*sysfs init*/

    /* uart init */
    Uart_t* ctx_uart_icr = (Uart_t*)malloc(sizeof(Uart_t));
    memset(ctx_uart_icr, 0, sizeof(Uart_t));

    /* uart params */ 
    ctx_uart_icr = Context_init_uart("/dev/ttyS5",115200,8,1,'N');

    uart_init(ctx_uart_icr, O_RDWR | O_NOCTTY | O_NONBLOCK);
    uart_show_data(ctx_uart_icr);

    /*cmd hex*/
    unsigned char SLP_ICR_CMD_SET_OPTION[18] = {0x55, 0xAA, 0x73, 0x95, 0x00, 0xA0, 
                                            0x10, 0x00, 0x30, 0x0D, 0x17, 0x00, 
                                            0x00, 0x00, 0x00, 0x05, 0x50, 0x61};

    unsigned char SLP_ICR_CMD_DETECTOR_POWERON[6]   = {0x55, 0xAA, 0x73, 0xB1, 0xB1,0xD5};
    unsigned char SLP_ICR_CMD_VPROCRCUT_POWERON[6]  = {0x55, 0xAA, 0x73, 0xB3, 0xB3,0xD7};
    unsigned char SLP_ICR_CMD_VIDEO_START[6]        = {0x55, 0xAA, 0x73, 0xB5, 0xB5,0xD9};

    // 定义命令数组和命令数量
    unsigned char* commands[] = {
        SLP_ICR_CMD_SET_OPTION,
        SLP_ICR_CMD_DETECTOR_POWERON,
        SLP_ICR_CMD_VPROCRCUT_POWERON,
        SLP_ICR_CMD_VIDEO_START
    };
    
    size_t command_sizes[] = {
        sizeof(SLP_ICR_CMD_SET_OPTION),
        sizeof(SLP_ICR_CMD_DETECTOR_POWERON),
        sizeof(SLP_ICR_CMD_VPROCRCUT_POWERON),
        sizeof(SLP_ICR_CMD_VIDEO_START)
    };
    
    const char* command_names[] = {
        "SET_OPTION",
        "DETECTOR_POWERON",
        "VPROCRCUT_POWERON",
        "VIDEO_START"
    };
    
    const int COMMAND_COUNT = 4;
    int current_command = 0;

    // 初始化select相关变量
    fd_set readfds;
    struct timeval timeout;
    int max_fd = ctx_uart_icr->fd + 1;
    unsigned char buf[256];

    // 发送当前命令
    printf("发送命令: %s\n", command_names[current_command]);
    SendCmdToSubdev("icr", ctx_uart_icr->fd, commands[current_command], command_sizes[current_command], 1000);
    // 循环发送命令并接收响应
    while (1)
    {

        // 准备select
        FD_ZERO(&readfds);
        FD_SET(ctx_uart_icr->fd, &readfds);

        // 设置超时时间
        timeout.tv_sec = 5;  // 5秒超时
        timeout.tv_usec = 0;

        // 调用select监控串口
        int activity = select(max_fd, &readfds, NULL, NULL, &timeout);

        if (activity < 0) {
            perror("select错误");
            break;
        } else if (activity == 0) {
            printf("接收超时，未收到响应\n");
        } else {
            // 有数据可读
            int n = read(ctx_uart_icr->fd, buf, sizeof(buf));
            printf("读取的返回字节：%d\n", n);
            if (n > 0) {
                // 从第0个位置开始查找同步码并处理
                int offset = 0;
                while (offset < n - 54) {
                    general_process_response(buf, n, offset);
                    
                    // 查找下一个可能的数据帧
                    // 简单策略：找到下一个同步码位置
                    int next_sync = -1;
                    for (int i = offset + 1; i < n - 1; i++) {
                        if (buf[i] == 0x55 && buf[i+1] == 0xAA) {
                            next_sync = i;
                            break;
                        }
                    }
                    
                    if (next_sync == -1) {
                        break;  // 没有更多同步码
                    }
                    
                    offset = next_sync;
                }
            }
        }

        // 切换到下一个命令
        // current_command = (current_command + 1) % COMMAND_COUNT;
        
        // 延时后再次发送
        usleep(SEND_INTERVAL * 1000);
    }
    
    uart_deinit(&ctx_uart_icr);
    free(ctx_uart_icr);  // 释放分配的内存
  
    return 0;
}

